/**
 * @file    SWDd_host.c
 * @brief   Host driver for accessing the DAP
 */
#include "stdio.h" 
#include "delay.h"
#include "swd_host.h"
#include "SWD_flash.h"
#include "DAP_config.h"
#include "DAP.h"
#include "debug_cm.h"

// AP CSW register, base value
#define CSW_VALUE (CSW_RESERVED | CSW_MSTRDBG | CSW_HPROT | CSW_DBGSTAT | CSW_SADDRINC)

// SWD register access
#define SWD_REG_AP        			(1)
#define SWD_REG_DP        			(0)
#define SWD_REG_R         			(1<<1)
#define SWD_REG_W         			(0<<1)
#define SWD_REG_ADR(a)    			(a & 0x0c)

#define DCRDR 						0xE000EDF8
#define DCRSR 						0xE000EDF4
#define DHCSR 						0xE000EDF0
#define REGWnR 						(1 << 16)

#define MAX_SWD_RETRY 				10
#define MAX_TIMEOUT   				10000

#define SCB_AIRCR_PRIGROUP_Pos      8U
#define SCB_AIRCR_PRIGROUP_Msk		(7UL << SCB_AIRCR_PRIGROUP_Pos)

extern SPI_HandleTypeDef hspi1;

typedef struct {
    uint32_t select;
    uint32_t csw;
} DAP_STATE;

typedef struct {
    uint32_t r[16];
    uint32_t xpsr;
} DEBUG_STATE;




//********************************************************************************
static DAP_STATE dap_state_1;

static uint8_t swd_read_core_register_1(uint32_t n, uint32_t *val);
static uint8_t swd_write_core_register_1(uint32_t n, uint32_t val);

static void int2array_1(uint8_t *res, uint32_t data, uint8_t len)
{
    uint8_t i = 0;

    for(i = 0; i < len; i++) {
        res[i] = (data >> 8 * i) & 0xFF;
    }
}

static uint8_t swd_transfer_retry_1(uint32_t req, uint32_t *data)
{
    uint8_t i, ack=0;

    for (i = 0; i < MAX_SWD_RETRY; i++) 
	{
        ack = SWD_Transfer_1(req, data);
        if (ack != DAP_TRANSFER_WAIT) 
		{
            return ack;
        }
    }
    return ack;
}

uint8_t swd_init_1(void)
{
    DAP_Setup_1();
    PORT_SWD_SETUP_1();
	
    return 1;
}

uint8_t swd_off_1(void)
{
    PORT_OFF_1();
	
    return 1;
}

// Read debug port register.
uint8_t swd_read_dp_1(uint8_t adr, uint32_t *val)
{
    uint32_t tmp_in;
    uint8_t tmp_out[4];
    uint8_t ack;
    uint32_t tmp;
	
    tmp_in = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(adr);
    ack = swd_transfer_retry_1(tmp_in, (uint32_t *)tmp_out);
	
    *val = 0;
    tmp = tmp_out[3];
    *val |= (tmp << 24);
    tmp = tmp_out[2];
    *val |= (tmp << 16);
    tmp = tmp_out[1];
    *val |= (tmp << 8);
    tmp = tmp_out[0];
    *val |= (tmp << 0);
	
    return (ack == 0x01);
}

// Write debug port register
uint8_t swd_write_dp_1(uint8_t adr, uint32_t val)
{
    uint32_t req;
    uint8_t data[4];
    uint8_t ack;

    switch (adr) {
        case DP_SELECT:
            if (dap_state_1.select == val) {
                return 1;
            }

            dap_state_1.select = val;
            break;

        default:
            break;
    }

    req = SWD_REG_DP | SWD_REG_W | SWD_REG_ADR(adr);
    int2array_1(data, val, 4);
    ack = swd_transfer_retry_1(req, (uint32_t *)data);
	
    return (ack == 0x01);
}

// Read access port register.
uint8_t swd_read_ap_1(uint32_t adr, uint32_t *val)
{
    uint8_t tmp_in, ack;
    uint8_t tmp_out[4];
    uint32_t tmp;
    uint32_t apsel = adr & 0xff000000;
    uint32_t bank_sel = adr & APBANKSEL;

    if (!swd_write_dp_1(DP_SELECT, apsel | bank_sel)) {
        return 0;
    }

    tmp_in = SWD_REG_AP | SWD_REG_R | SWD_REG_ADR(adr);
    // first dummy read
    swd_transfer_retry_1(tmp_in, (uint32_t *)tmp_out);
    ack = swd_transfer_retry_1(tmp_in, (uint32_t *)tmp_out);
	
    *val = 0;
    tmp = tmp_out[3];
    *val |= (tmp << 24);
    tmp = tmp_out[2];
    *val |= (tmp << 16);
    tmp = tmp_out[1];
    *val |= (tmp << 8);
    tmp = tmp_out[0];
    *val |= (tmp << 0);
	
    return (ack == 0x01);
}

// Write access port register
uint8_t swd_write_ap_1(uint32_t adr, uint32_t val)
{
    uint8_t data[4];
    uint8_t req, ack;
    uint32_t apsel = adr & 0xff000000;
    uint32_t bank_sel = adr & APBANKSEL;
	
	if(g_AP_SEL_1 == M0_AP)
	{
		apsel |= 0x03000000;
	}

    if (!swd_write_dp_1(DP_SELECT, apsel | bank_sel)) {
        return 0;
    }

    switch (adr) {
        case AP_CSW:
            if (dap_state_1.csw == val) {
                return 1;
            }

            dap_state_1.csw = val;
            break;

        default:
            break;
    }

    req = SWD_REG_AP | SWD_REG_W | SWD_REG_ADR(adr);
    int2array_1(data, val, 4);

    if (swd_transfer_retry_1(req, (uint32_t *)data) != 0x01) {
        return 0;
    }

    req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
    ack = swd_transfer_retry_1(req, NULL);
	
    return (ack == 0x01);
}

// Write 32-bit word aligned values to target memory using address auto-increment.
// size is in bytes.
static uint8_t swd_write_block_1(uint32_t address, uint8_t *data, uint32_t size)
{
    uint8_t tmp_in[4], req;
    uint32_t size_in_words;
    uint32_t i, ack;

    if (size == 0) {
        return 0;
    }

    size_in_words = size / 4;

    // CSW register
    if (!swd_write_ap_1(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
        return 0;
    }

    // TAR write
    req = SWD_REG_AP | SWD_REG_W | (1 << 2);
    int2array_1(tmp_in, address, 4);

    if (swd_transfer_retry_1(req, (uint32_t *)tmp_in) != 0x01) {
        return 0;
    }

    // DRW write
    req = SWD_REG_AP | SWD_REG_W | (3 << 2);

    for (i = 0; i < size_in_words; i++) {
        if (swd_transfer_retry_1(req, (uint32_t *)data) != 0x01) {
            return 0;
        }

        data += 4;
    }

    // dummy read
    req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
    ack = swd_transfer_retry_1(req, NULL);
    return (ack == 0x01);
}

// Read 32-bit word aligned values from target memory using address auto-increment.
// size is in bytes.
static uint8_t swd_read_block_1(uint32_t address, uint8_t *data, uint32_t size)
{
    uint8_t tmp_in[4], req, ack;
    uint32_t size_in_words;
    uint32_t i;

    if (size == 0) {
        return 0;
    }

    size_in_words = size / 4;

    if (!swd_write_ap_1(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
        return 0;
    }

    // TAR write
    req = SWD_REG_AP | SWD_REG_W | AP_TAR;
    int2array_1(tmp_in, address, 4);

    if (swd_transfer_retry_1(req, (uint32_t *)tmp_in) != DAP_TRANSFER_OK) {
        return 0;
    }

    // read data
    req = SWD_REG_AP | SWD_REG_R | AP_DRW;

    // initiate first read, data comes back in next read
    if (swd_transfer_retry_1(req, NULL) != 0x01) {
        return 0;
    }

    for (i = 0; i < (size_in_words - 1); i++) {
        if (swd_transfer_retry_1(req, (uint32_t *)data) != DAP_TRANSFER_OK) {
            return 0;
        }

        data += 4;
    }

    // read last word
    req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
    ack = swd_transfer_retry_1(req, (uint32_t *)data);
    return (ack == 0x01);
}

// Read target memory.
uint8_t swd_read_data_1(uint32_t addr, uint32_t *val)
{
    uint8_t tmp_in[4];
    uint8_t tmp_out[4];
    uint8_t req, ack;
    uint32_t tmp;
    // put addr in TAR register
    int2array_1(tmp_in, addr, 4);
    req = SWD_REG_AP | SWD_REG_W | (1 << 2);

    if (swd_transfer_retry_1(req, (uint32_t *)tmp_in) != 0x01) {
        return 0;
    }

    // read data
    req = SWD_REG_AP | SWD_REG_R | (3 << 2);

    if (swd_transfer_retry_1(req, (uint32_t *)tmp_out) != 0x01) {
        return 0;
    }

    // dummy read
    req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
    ack = swd_transfer_retry_1(req, (uint32_t *)tmp_out);
    *val = 0;
    tmp = tmp_out[3];
    *val |= (tmp << 24);
    tmp = tmp_out[2];
    *val |= (tmp << 16);
    tmp = tmp_out[1];
    *val |= (tmp << 8);
    tmp = tmp_out[0];
    *val |= (tmp << 0);
    return (ack == 0x01);
}

// Write target memory.
uint8_t swd_write_data_1(uint32_t address, uint32_t data)
{
    uint8_t tmp_in[4];
    uint8_t req, ack;
    // put addr in TAR register
    int2array_1(tmp_in, address, 4);
    req = SWD_REG_AP | SWD_REG_W | (1 << 2);

    if (swd_transfer_retry_1(req, (uint32_t *)tmp_in) != 0x01) {
        return 0;
    }

    // write data
    int2array_1(tmp_in, data, 4);
    req = SWD_REG_AP | SWD_REG_W | (3 << 2);

    if (swd_transfer_retry_1(req, (uint32_t *)tmp_in) != 0x01) {
        return 0;
    }

    // dummy read
    req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
    ack = swd_transfer_retry_1(req, NULL);
    return ack;
}

// Read 32-bit word from target memory.
uint8_t swd_read_word_1(uint32_t addr, uint32_t *val)
{
    if (!swd_write_ap_1(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
        return 0;
    }
		
    if (!swd_read_data_1(addr, val)) {
        return 0;
    }
    return 1;
}

// Write 32-bit word to target memory.
uint8_t swd_write_word_1(uint32_t addr, uint32_t val)
{
    if (!swd_write_ap_1(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
        return 0;
    }


    if (!swd_write_data_1(addr, val)) {
        return 0;
    }
		
    return 1;
}

// Read 8-bit byte from target memory.
static uint8_t swd_read_byte_1(uint32_t addr, uint8_t *val)
{
    uint32_t tmp;

    if (!swd_write_ap_1(AP_CSW, CSW_VALUE | CSW_SIZE8)) {
        return 0;
    }

    if (!swd_read_data_1(addr, &tmp)) {
        return 0;
    }

    *val = (uint8_t)(tmp >> ((addr & 0x03) << 3));
    return 1;
}

// Write 8-bit byte to target memory.
static uint8_t swd_write_byte_1(uint32_t addr, uint8_t val)
{
    uint32_t tmp;

    if (!swd_write_ap_1(AP_CSW, CSW_VALUE | CSW_SIZE8)) {
        return 0;
    }

    tmp = (uint32_t)val << ((addr & 0x03) << 3);

    if (!swd_write_data_1(addr, tmp)) {
        return 0;
    }

    return 1;
}

// Read unaligned data from target memory.
// size is in bytes.
uint8_t swd_read_memory(uint32_t address, uint8_t *data, uint32_t size)
{
    uint32_t n;

    // Read bytes until word aligned
    while ((size > 0) && (address & 0x3)) {
        if (!swd_read_byte_1(address, data)) {
            return 0;
        }

        address++;
        data++;
        size--;
    }

    // Read word aligned blocks
    while (size > 3) {
        // Limit to auto increment page size
        n = Flash_Page_Size - (address & (Flash_Page_Size - 1));

        if (size < n) {
            n = size & 0xFFFFFFFC; // Only count complete words remaining
        }

        if (!swd_read_block_1(address, data, n)) {
            return 0;
        }

        address += n;
        data += n;
        size -= n;
    }

    // Read remaining bytes
    while (size > 0) {
        if (!swd_read_byte_1(address, data)) {
            return 0;
        }

        address++;
        data++;
        size--;
    }

    return 1;
}

// Write unaligned data to target memory.
// size is in bytes.
uint8_t swd_write_memory_1(uint32_t address, uint8_t *data, uint32_t size)
{
    uint32_t n = 0;

    // Write bytes until word aligned
    while ((size > 0) && (address & 0x3)) {
        if (!swd_write_byte_1(address, *data)) {
            return 0;
        }

        address++;
        data++;
        size--;
    }

    // Write word aligned blocks
    while (size > 3) {
        // Limit to auto increment page size
        n = Flash_Page_Size - (address & (Flash_Page_Size - 1));

        if (size < n) {
            n = size & 0xFFFFFFFC; // Only count complete words remaining
        }

        if (!swd_write_block_1(address, data, n)) {
            return 0;
        }

        address += n;
        data += n;
        size -= n;
    }

    // Write remaining bytes
    while (size > 0) {
        if (!swd_write_byte_1(address, *data)) {
            return 0;
        }

        address++;
        data++;
        size--;
    }

    return 1;
}

// Execute system call.
static uint8_t swd_write_debug_state_1(DEBUG_STATE *state)
{
    uint32_t i, status;

    if (!swd_write_dp_1(DP_SELECT, 0)) {
        return 0;
    }

    // R0, R1, R2, R3
    for (i = 0; i < 4; i++) {
        if (!swd_write_core_register_1(i, state->r[i])) {
            return 0;
        }
    }

    // R9
    if (!swd_write_core_register_1(9, state->r[9])) {
        return 0;
    }

    // R13, R14, R15
    for (i = 13; i < 16; i++) {
        if (!swd_write_core_register_1(i, state->r[i])) {
            return 0;
        }
    }

    // xPSR
    if (!swd_write_core_register_1(16, state->xpsr)) {
        return 0;
    }

    if (!swd_write_word_1(DBG_HCSR, DBGKEY | C_DEBUGEN)) {
        return 0;
    }

    // check status
    if (!swd_read_dp_1(DP_CTRL_STAT, &status)) {
        return 0;
    }

    if (status & (STICKYERR | WDATAERR)) {
        return 0;
    }

    return 1;
}

static uint8_t swd_read_core_register_1(uint32_t n, uint32_t *val)
{
    int i = 0, timeout = 100;

    if (!swd_write_word_1(DCRSR, n)) {
        return 0;
    }

    // wait for S_REGRDY
    for (i = 0; i < timeout; i++) {
        if (!swd_read_word_1(DHCSR, val)) {
            return 0;
        }

        if (*val & S_REGRDY) {
            break;
        }
    }

    if (i == timeout) {
        return 0;
    }

    if (!swd_read_word_1(DCRDR, val)) {
        return 0;
    }

    return 1;
}

static uint8_t swd_write_core_register_1(uint32_t n, uint32_t val)
{
    int i = 0, timeout = 100;

    if (!swd_write_word_1(DCRDR, val)) {
        return 0;
    }

    if (!swd_write_word_1(DCRSR, n | REGWnR)) {
        return 0;
    }

    // wait for S_REGRDY
    for (i = 0; i < timeout; i++) {
        if (!swd_read_word_1(DHCSR, &val)) {
            return 0;
        }

        if (val & S_REGRDY) {
            return 1;
        }
    }

    return 0;
}

static uint8_t swd_wait_until_halted_1(void)
{
    // Wait for target to stop
    uint32_t val, i, timeout = MAX_TIMEOUT;
	
	#ifdef exec_try
		if (syscall_exec_try_1 == 1)
		{
		timeout = 5; 	
		}
	#endif

    for (i = 0; i < timeout; i++) {
		
        if (!swd_read_word_1(DBG_HCSR, &val)) {
            return 0;
        }

        if (val & S_HALT) {
			
			#ifdef exec_try
				syscall_exec_try_1 = 0;	
			#endif
			
            return 1;
        }
    }

    return 0;
}

uint8_t swd_flash_syscall_exec_1(const program_syscall_t *sysCallParam, uint32_t entry, uint32_t arg1, uint32_t arg2, uint32_t arg3, uint32_t arg4)
{
    DEBUG_STATE state = {{0}, 0};
    // Call flash algorithm function on target and wait for result.
    state.r[0]     = arg1;                   // R0: Argument 1
    state.r[1]     = arg2;                   // R1: Argument 2
    state.r[2]     = arg3;                   // R2: Argument 3
    state.r[3]     = arg4;                   // R3: Argument 4
    state.r[9]     = sysCallParam->static_base;    // SB: Static Base
    state.r[13]    = sysCallParam->stack_pointer;  // SP: Stack Pointer
    state.r[14]    = sysCallParam->breakpoint;     // LR: Exit Point
    state.r[15]    = entry;                        // PC: Entry Point
    state.xpsr     = 0x01000000;          // xPSR: T = 1, ISR = 0
		
    if (!swd_write_debug_state_1(&state)) {
		Debug("Write State Faulf\r\n");
        return 0;
    }

    if (!swd_wait_until_halted_1()) {
		#ifdef exec_try
			if (syscall_exec_try_1 == 1)
			{
				syscall_exec_try_1 = 0;	
				
				return 0;
			}
			else
			{
				Debug("swd_wait_until_halted_1 Faulf\r\n");
				
				return 0;
			}	
		#endif
		
		Debug("swd_wait_until_halted_1 Faulf\r\n");
			
        return 0;
    }

    if (!swd_read_core_register_1(0, &state.r[0])) {
		Debug("swd_read_core_register_1 Faulf\r\n");
        return 0;
    }

    // Flash functions return 0 if successful.
    if (state.r[0] != 0) {
		Debug(" Fault 0x%.4X\r\n",state.r[0]);
       return 0;
    }

    return 1;
}

// SWD Reset
static uint8_t swd_reset_1(void)
{
    uint8_t tmp_in[8];
    uint8_t i = 0;

    for (i = 0; i < 8; i++) {
        tmp_in[i] = 0xff;
    }

    SWD_Sequence_1(51, tmp_in);
    return 1;
}

// SWD Switch
static uint8_t swd_switch_1(uint16_t val)
{
    uint8_t tmp_in[2];
    tmp_in[0] = val & 0xff;
    tmp_in[1] = (val >> 8) & 0xff;
    SWD_Sequence_1(16, tmp_in);
    return 1;
}

// SWD Read ID
static uint8_t swd_read_idcode_1(uint32_t *id)
{
    uint8_t tmp_in[1];
    uint8_t tmp_out[4];
    tmp_in[0] = 0x00;
    SWD_Sequence_1(8, tmp_in);
	
    if (swd_read_dp_1(0, (uint32_t *)tmp_out) != 0x01) {
        return 0;
    }

    *id = (tmp_out[3] << 24) | (tmp_out[2] << 16) | (tmp_out[1] << 8) | tmp_out[0];
    return 1;
}

static uint8_t JTAG2SWD_1()
{
    uint32_t tmp = 0;

    if (!swd_reset_1()) {
        return 0;
    }

    if (!swd_switch_1(0xE79E)) {
        return 0;
    }

    if (!swd_reset_1()) {
        return 0;
    }
		
    if (!swd_switch_1(0xB76D)) {
        return 0;
    }

    if (!swd_reset_1()) {
        return 0;
    }

    if (!swd_read_idcode_1(&tmp)) {
		Debug("Read IDCODE Fault 0x%.4X\r\n",tmp);
        return 0;
    }
		
	Debug("Read IDCODE 0x%.4X\r\n",tmp);
    return 1;
}

uint8_t swd_init_debug_1(void)
{
    uint32_t tmp = 0;
    int i = 0;
    uint8_t timeout = 100;
    // init dap state with fake values
    dap_state_1.select = 0xffffffff;
    dap_state_1.csw = 0xffffffff;
	
    swd_init_1();
	
    // call a target dependant function
    // this function can do several stuff before really initing the debug
    //target_before_init_debug();

    if (!JTAG2SWD_1()) {
		Debug("JTAG2SWD_1 Fault\r\n");
        return 0;
    }
		
    if (!swd_write_dp_1(DP_ABORT, STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR)) {
		Debug("swd_write_dp_1 Fault\r\n");
        return 0;
    }

    // Ensure CTRL/STAT register selected in DPBANKSEL
    if (!swd_write_dp_1(DP_SELECT, 0)) {
		Debug("swd_write_dp_1 DP_SELECT Fault\r\n");
        return 0;
    }

    // Power up
    if (!swd_write_dp_1(DP_CTRL_STAT, CSYSPWRUPREQ | CDBGPWRUPREQ)) {
		Debug("swd_write_dp_1 DP_CTRL_STAT Fault\r\n");
        return 0;
    }

    for (i = 0; i < timeout; i++) {
        if (!swd_read_dp_1(DP_CTRL_STAT, &tmp)) {
			Debug("swd_read_dp_1 DP_CTRL_STAT Fault\r\n");
            return 0;
        }
        if ((tmp & (CDBGPWRUPACK | CSYSPWRUPACK)) == (CDBGPWRUPACK | CSYSPWRUPACK)) {
            // Break from loop if powerup is complete
            break;
        }
    }
    if (i == timeout) {
		Debug("Unable to powerup DP\r\n");
        // Unable to powerup DP
        return 0;
    }

    if (!swd_write_dp_1(DP_CTRL_STAT, CSYSPWRUPREQ | CDBGPWRUPREQ | TRNNORMAL | MASKLANE)) {
		Debug("swd_write_dp_1 DP_CTRL_STAT Fault\r\n");
        return 0;
    }
    // call a target dependant function:
    // some target can enter in a lock state, this function can unlock these targets
    //target_unlock_sequence();

    if (!swd_write_dp_1(DP_SELECT, 0)) {
		Debug("swd_write_dp_1 DP_SELECT last Fault\r\n");
        return 0;
    }
    return 1;
}

void swd_set_target_reset_hw_1(uint8_t asserted)
{
    (asserted) ? PIN_nRESET_OUT_1(0) : PIN_nRESET_OUT_1(1);
}

void swd_set_target_reset_sw_1(void)
{
	if(g_AP_SEL_1 == M0_AP)
	{
		swd_write_word_1((uint32_t)&SCB->AIRCR, ((0x5FA << SCB_AIRCR_VECTKEY_Pos) |(SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) | SCB_AIRCR_SYSRESETREQ_Msk));
	}
	else
	{
		swd_write_word_1((uint32_t)&SCB->AIRCR, ((0x5FA << SCB_AIRCR_VECTKEY_Pos) |(SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) | SCB_AIRCR_VECTCLRACTIVE_Msk));
	}
}

uint8_t swd_set_target_state_hw_1(TARGET_RESET_STATE state)
{
    uint32_t val;
    int8_t ap_retries = 2;
    /* Calling swd_init_1 prior to entering RUN state causes operations to fail. */
    if (state != RUN) {
        swd_init_1();
    }

    switch (state) {
        case RESET_HOLD:
            swd_set_target_reset_hw_1(1);
            break;

        case RESET_RUN:
            swd_set_target_reset_hw_1(1);
            delay_ms(20);
            swd_set_target_reset_hw_1(0);
            delay_ms(20);
            swd_off_1();
            break;

        case RESET_PROGRAM:
            if (!swd_init_debug_1()) {
                return 0;
            }
												
            // Enable debug
            while(swd_write_word_1(DBG_HCSR, DBGKEY | C_DEBUGEN | C_HALT) == 0) 
			{
                if( --ap_retries <=0 )
				{
					Debug("write DBG_HCSR Fault\r\n");
					return 0;
				}                
            }
						
            do
			{
                if (!swd_read_word_1(DBG_HCSR, &val))
				{
					Debug("swd_read_word_1 Fault 0x%.4X\r\n",val);
					return 0;
                }	
				swd_set_target_reset_sw_1();
			} 
			while ((val & S_HALT) == 0);

            break;

        default:
            return 0;
    }

    return 1;
}




//********************************************************************************
static DAP_STATE dap_state_2;

static uint8_t swd_read_core_register_2(uint32_t n, uint32_t *val);
static uint8_t swd_write_core_register_2(uint32_t n, uint32_t val);

static void int2array_2(uint8_t *res, uint32_t data, uint8_t len)
{
    uint8_t i = 0;

    for(i = 0; i < len; i++) {
        res[i] = (data >> 8 * i) & 0xFF;
    }
}

static uint8_t swd_transfer_retry_2(uint32_t req, uint32_t *data)
{
    uint8_t i, ack=0;

    for (i = 0; i < MAX_SWD_RETRY; i++) 
	{
        ack = SWD_Transfer_2(req, data);
        if (ack != DAP_TRANSFER_WAIT) 
		{
            return ack;
        }
    }
    return ack;
}

uint8_t swd_init_2(void)
{
    DAP_Setup_2();
    PORT_SWD_SETUP_2();
	
    return 1;
}

uint8_t swd_off_2(void)
{
    PORT_OFF_2();
	
    return 1;
}

// Read debug port register.
uint8_t swd_read_dp_2(uint8_t adr, uint32_t *val)
{
    uint32_t tmp_in;
    uint8_t tmp_out[4];
    uint8_t ack;
    uint32_t tmp;
	
    tmp_in = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(adr);
    ack = swd_transfer_retry_2(tmp_in, (uint32_t *)tmp_out);
	
    *val = 0;
    tmp = tmp_out[3];
    *val |= (tmp << 24);
    tmp = tmp_out[2];
    *val |= (tmp << 16);
    tmp = tmp_out[1];
    *val |= (tmp << 8);
    tmp = tmp_out[0];
    *val |= (tmp << 0);
	
    return (ack == 0x01);
}

// Write debug port register
uint8_t swd_write_dp_2(uint8_t adr, uint32_t val)
{
    uint32_t req;
    uint8_t data[4];
    uint8_t ack;

    switch (adr) {
        case DP_SELECT:
            if (dap_state_2.select == val) {
                return 1;
            }

            dap_state_2.select = val;
            break;

        default:
            break;
    }

    req = SWD_REG_DP | SWD_REG_W | SWD_REG_ADR(adr);
    int2array_2(data, val, 4);
    ack = swd_transfer_retry_2(req, (uint32_t *)data);
	
    return (ack == 0x01);
}

// Read access port register.
uint8_t swd_read_ap_2(uint32_t adr, uint32_t *val)
{
    uint8_t tmp_in, ack;
    uint8_t tmp_out[4];
    uint32_t tmp;
    uint32_t apsel = adr & 0xff000000;
    uint32_t bank_sel = adr & APBANKSEL;

    if (!swd_write_dp_2(DP_SELECT, apsel | bank_sel)) {
        return 0;
    }

    tmp_in = SWD_REG_AP | SWD_REG_R | SWD_REG_ADR(adr);
    // first dummy read
    swd_transfer_retry_2(tmp_in, (uint32_t *)tmp_out);
    ack = swd_transfer_retry_2(tmp_in, (uint32_t *)tmp_out);
	
    *val = 0;
    tmp = tmp_out[3];
    *val |= (tmp << 24);
    tmp = tmp_out[2];
    *val |= (tmp << 16);
    tmp = tmp_out[1];
    *val |= (tmp << 8);
    tmp = tmp_out[0];
    *val |= (tmp << 0);
	
    return (ack == 0x01);
}

// Write access port register
uint8_t swd_write_ap_2(uint32_t adr, uint32_t val)
{
    uint8_t data[4];
    uint8_t req, ack;
    uint32_t apsel = adr & 0xff000000;
    uint32_t bank_sel = adr & APBANKSEL;
	
	if(g_AP_SEL_2 == M0_AP)
	{
		apsel |= 0x03000000;
	}

    if (!swd_write_dp_2(DP_SELECT, apsel | bank_sel)) {
        return 0;
    }

    switch (adr) {
        case AP_CSW:
            if (dap_state_2.csw == val) {
                return 1;
            }

            dap_state_2.csw = val;
            break;

        default:
            break;
    }

    req = SWD_REG_AP | SWD_REG_W | SWD_REG_ADR(adr);
    int2array_2(data, val, 4);

    if (swd_transfer_retry_2(req, (uint32_t *)data) != 0x01) {
        return 0;
    }

    req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
    ack = swd_transfer_retry_2(req, NULL);
	
    return (ack == 0x01);
}

// Write 32-bit word aligned values to target memory using address auto-increment.
// size is in bytes.
static uint8_t swd_write_block_2(uint32_t address, uint8_t *data, uint32_t size)
{
    uint8_t tmp_in[4], req;
    uint32_t size_in_words;
    uint32_t i, ack;

    if (size == 0) {
        return 0;
    }

    size_in_words = size / 4;

    // CSW register
    if (!swd_write_ap_2(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
        return 0;
    }

    // TAR write
    req = SWD_REG_AP | SWD_REG_W | (1 << 2);
    int2array_2(tmp_in, address, 4);

    if (swd_transfer_retry_2(req, (uint32_t *)tmp_in) != 0x01) {
        return 0;
    }

    // DRW write
    req = SWD_REG_AP | SWD_REG_W | (3 << 2);

    for (i = 0; i < size_in_words; i++) {
        if (swd_transfer_retry_2(req, (uint32_t *)data) != 0x01) {
            return 0;
        }

        data += 4;
    }

    // dummy read
    req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
    ack = swd_transfer_retry_2(req, NULL);
    return (ack == 0x01);
}

// Read 32-bit word aligned values from target memory using address auto-increment.
// size is in bytes.
static uint8_t swd_read_block_2(uint32_t address, uint8_t *data, uint32_t size)
{
    uint8_t tmp_in[4], req, ack;
    uint32_t size_in_words;
    uint32_t i;

    if (size == 0) {
        return 0;
    }

    size_in_words = size / 4;

    if (!swd_write_ap_2(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
        return 0;
    }

    // TAR write
    req = SWD_REG_AP | SWD_REG_W | AP_TAR;
    int2array_2(tmp_in, address, 4);

    if (swd_transfer_retry_2(req, (uint32_t *)tmp_in) != DAP_TRANSFER_OK) {
        return 0;
    }

    // read data
    req = SWD_REG_AP | SWD_REG_R | AP_DRW;

    // initiate first read, data comes back in next read
    if (swd_transfer_retry_2(req, NULL) != 0x01) {
        return 0;
    }

    for (i = 0; i < (size_in_words - 1); i++) {
        if (swd_transfer_retry_2(req, (uint32_t *)data) != DAP_TRANSFER_OK) {
            return 0;
        }

        data += 4;
    }

    // read last word
    req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
    ack = swd_transfer_retry_2(req, (uint32_t *)data);
    return (ack == 0x01);
}

// Read target memory.
uint8_t swd_read_data_2(uint32_t addr, uint32_t *val)
{
    uint8_t tmp_in[4];
    uint8_t tmp_out[4];
    uint8_t req, ack;
    uint32_t tmp;
    // put addr in TAR register
    int2array_2(tmp_in, addr, 4);
    req = SWD_REG_AP | SWD_REG_W | (1 << 2);

    if (swd_transfer_retry_2(req, (uint32_t *)tmp_in) != 0x01) {
        return 0;
    }

    // read data
    req = SWD_REG_AP | SWD_REG_R | (3 << 2);

    if (swd_transfer_retry_2(req, (uint32_t *)tmp_out) != 0x01) {
        return 0;
    }

    // dummy read
    req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
    ack = swd_transfer_retry_2(req, (uint32_t *)tmp_out);
    *val = 0;
    tmp = tmp_out[3];
    *val |= (tmp << 24);
    tmp = tmp_out[2];
    *val |= (tmp << 16);
    tmp = tmp_out[1];
    *val |= (tmp << 8);
    tmp = tmp_out[0];
    *val |= (tmp << 0);
    return (ack == 0x01);
}

// Write target memory.
uint8_t swd_write_data_2(uint32_t address, uint32_t data)
{
    uint8_t tmp_in[4];
    uint8_t req, ack;
    // put addr in TAR register
    int2array_2(tmp_in, address, 4);
    req = SWD_REG_AP | SWD_REG_W | (1 << 2);

    if (swd_transfer_retry_2(req, (uint32_t *)tmp_in) != 0x01) {
        return 0;
    }

    // write data
    int2array_2(tmp_in, data, 4);
    req = SWD_REG_AP | SWD_REG_W | (3 << 2);

    if (swd_transfer_retry_2(req, (uint32_t *)tmp_in) != 0x01) {
        return 0;
    }

    // dummy read
    req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
    ack = swd_transfer_retry_2(req, NULL);
    return ack;
}

// Read 32-bit word from target memory.
uint8_t swd_read_word_2(uint32_t addr, uint32_t *val)
{
    if (!swd_write_ap_2(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
        return 0;
    }
		
    if (!swd_read_data_2(addr, val)) {
        return 0;
    }
    return 1;
}

// Write 32-bit word to target memory.
uint8_t swd_write_word_2(uint32_t addr, uint32_t val)
{
    if (!swd_write_ap_2(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
        return 0;
    }


    if (!swd_write_data_2(addr, val)) {
        return 0;
    }
		
    return 1;
}

// Read 8-bit byte from target memory.
static uint8_t swd_read_byte_2(uint32_t addr, uint8_t *val)
{
    uint32_t tmp;

    if (!swd_write_ap_2(AP_CSW, CSW_VALUE | CSW_SIZE8)) {
        return 0;
    }

    if (!swd_read_data_2(addr, &tmp)) {
        return 0;
    }

    *val = (uint8_t)(tmp >> ((addr & 0x03) << 3));
    return 1;
}

// Write 8-bit byte to target memory.
static uint8_t swd_write_byte_2(uint32_t addr, uint8_t val)
{
    uint32_t tmp;

    if (!swd_write_ap_2(AP_CSW, CSW_VALUE | CSW_SIZE8)) {
        return 0;
    }

    tmp = (uint32_t)val << ((addr & 0x03) << 3);

    if (!swd_write_data_2(addr, tmp)) {
        return 0;
    }

    return 1;
}

// Read unaligned data from target memory.
// size is in bytes.
uint8_t swd_read_memory_2(uint32_t address, uint8_t *data, uint32_t size)
{
    uint32_t n;

    // Read bytes until word aligned
    while ((size > 0) && (address & 0x3)) {
        if (!swd_read_byte_2(address, data)) {
            return 0;
        }

        address++;
        data++;
        size--;
    }

    // Read word aligned blocks
    while (size > 3) {
        // Limit to auto increment page size
        n = Flash_Page_Size - (address & (Flash_Page_Size - 1));

        if (size < n) {
            n = size & 0xFFFFFFFC; // Only count complete words remaining
        }

        if (!swd_read_block_2(address, data, n)) {
            return 0;
        }

        address += n;
        data += n;
        size -= n;
    }

    // Read remaining bytes
    while (size > 0) {
        if (!swd_read_byte_2(address, data)) {
            return 0;
        }

        address++;
        data++;
        size--;
    }

    return 1;
}

// Write unaligned data to target memory.
// size is in bytes.
uint8_t swd_write_memory_2(uint32_t address, uint8_t *data, uint32_t size)
{
    uint32_t n = 0;

    // Write bytes until word aligned
    while ((size > 0) && (address & 0x3)) {
        if (!swd_write_byte_2(address, *data)) {
            return 0;
        }

        address++;
        data++;
        size--;
    }

    // Write word aligned blocks
    while (size > 3) {
        // Limit to auto increment page size
        n = Flash_Page_Size - (address & (Flash_Page_Size - 1));

        if (size < n) {
            n = size & 0xFFFFFFFC; // Only count complete words remaining
        }

        if (!swd_write_block_2(address, data, n)) {
            return 0;
        }

        address += n;
        data += n;
        size -= n;
    }

    // Write remaining bytes
    while (size > 0) {
        if (!swd_write_byte_2(address, *data)) {
            return 0;
        }

        address++;
        data++;
        size--;
    }

    return 1;
}

// Execute system call.
static uint8_t swd_write_debug_state_2(DEBUG_STATE *state)
{
    uint32_t i, status;

    if (!swd_write_dp_2(DP_SELECT, 0)) {
        return 0;
    }

    // R0, R1, R2, R3
    for (i = 0; i < 4; i++) {
        if (!swd_write_core_register_2(i, state->r[i])) {
            return 0;
        }
    }

    // R9
    if (!swd_write_core_register_2(9, state->r[9])) {
        return 0;
    }

    // R13, R14, R15
    for (i = 13; i < 16; i++) {
        if (!swd_write_core_register_2(i, state->r[i])) {
            return 0;
        }
    }

    // xPSR
    if (!swd_write_core_register_2(16, state->xpsr)) {
        return 0;
    }

    if (!swd_write_word_2(DBG_HCSR, DBGKEY | C_DEBUGEN)) {
        return 0;
    }

    // check status
    if (!swd_read_dp_2(DP_CTRL_STAT, &status)) {
        return 0;
    }

    if (status & (STICKYERR | WDATAERR)) {
        return 0;
    }

    return 1;
}

static uint8_t swd_read_core_register_2(uint32_t n, uint32_t *val)
{
    int i = 0, timeout = 100;

    if (!swd_write_word_2(DCRSR, n)) {
        return 0;
    }

    // wait for S_REGRDY
    for (i = 0; i < timeout; i++) {
        if (!swd_read_word_2(DHCSR, val)) {
            return 0;
        }

        if (*val & S_REGRDY) {
            break;
        }
    }

    if (i == timeout) {
        return 0;
    }

    if (!swd_read_word_2(DCRDR, val)) {
        return 0;
    }

    return 1;
}

static uint8_t swd_write_core_register_2(uint32_t n, uint32_t val)
{
    int i = 0, timeout = 100;

    if (!swd_write_word_2(DCRDR, val)) {
        return 0;
    }

    if (!swd_write_word_2(DCRSR, n | REGWnR)) {
        return 0;
    }

    // wait for S_REGRDY
    for (i = 0; i < timeout; i++) {
        if (!swd_read_word_2(DHCSR, &val)) {
            return 0;
        }

        if (val & S_REGRDY) {
            return 1;
        }
    }

    return 0;
}

static uint8_t swd_wait_until_halted_2(void)
{
    // Wait for target to stop
    uint32_t val, i, timeout = MAX_TIMEOUT;
	
	#ifdef exec_try
		if (syscall_exec_try_2 == 1)
		{
		timeout = 5; 	
		}
	#endif

    for (i = 0; i < timeout; i++) {
		
        if (!swd_read_word_2(DBG_HCSR, &val)) {
            return 0;
        }

        if (val & S_HALT) {
			
			#ifdef exec_try
				syscall_exec_try_2 = 0;	
			#endif
			
            return 1;
        }
    }

    return 0;
}

uint8_t swd_flash_syscall_exec_2(const program_syscall_t *sysCallParam, uint32_t entry, uint32_t arg1, uint32_t arg2, uint32_t arg3, uint32_t arg4)
{
    DEBUG_STATE state = {{0}, 0};
    // Call flash algorithm function on target and wait for result.
    state.r[0]     = arg1;                   // R0: Argument 1
    state.r[1]     = arg2;                   // R1: Argument 2
    state.r[2]     = arg3;                   // R2: Argument 3
    state.r[3]     = arg4;                   // R3: Argument 4
    state.r[9]     = sysCallParam->static_base;    // SB: Static Base
    state.r[13]    = sysCallParam->stack_pointer;  // SP: Stack Pointer
    state.r[14]    = sysCallParam->breakpoint;     // LR: Exit Point
    state.r[15]    = entry;                        // PC: Entry Point
    state.xpsr     = 0x01000000;          // xPSR: T = 1, ISR = 0
		
    if (!swd_write_debug_state_2(&state)) {
		Debug("Write State Faulf\r\n");
        return 0;
    }

    if (!swd_wait_until_halted_2()) {
		#ifdef exec_try
			if (syscall_exec_try_2 == 1)
			{
				syscall_exec_try_2 = 0;	
				
				return 0;
			}
			else
			{
				Debug("swd_wait_until_halted_2 Faulf\r\n");
				
				return 0;
			}	
		#endif
		
		Debug("swd_wait_until_halted_2 Faulf\r\n");
			
        return 0;
    }

    if (!swd_read_core_register_2(0, &state.r[0])) {
		Debug("swd_read_core_register_2 Faulf\r\n");
        return 0;
    }

    // Flash functions return 0 if successful.
    if (state.r[0] != 0) {
		Debug(" Fault 0x%.4X\r\n",state.r[0]);
       return 0;
    }

    return 1;
}

// SWD Reset
static uint8_t swd_reset_2(void)
{
    uint8_t tmp_in[8];
    uint8_t i = 0;

    for (i = 0; i < 8; i++) {
        tmp_in[i] = 0xff;
    }

    SWD_Sequence_2(51, tmp_in);
    return 1;
}

// SWD Switch
static uint8_t swd_switch_2(uint16_t val)
{
    uint8_t tmp_in[2];
    tmp_in[0] = val & 0xff;
    tmp_in[1] = (val >> 8) & 0xff;
    SWD_Sequence_2(16, tmp_in);
    return 1;
}

// SWD Read ID
static uint8_t swd_read_idcode_2(uint32_t *id)
{
    uint8_t tmp_in[1];
    uint8_t tmp_out[4];
    tmp_in[0] = 0x00;
    SWD_Sequence_2(8, tmp_in);
	
    if (swd_read_dp_2(0, (uint32_t *)tmp_out) != 0x01) {
        return 0;
    }

    *id = (tmp_out[3] << 24) | (tmp_out[2] << 16) | (tmp_out[1] << 8) | tmp_out[0];
    return 1;
}

static uint8_t JTAG2SWD_2()
{
    uint32_t tmp = 0;

    if (!swd_reset_2()) {
        return 0;
    }

    if (!swd_switch_2(0xE79E)) {
        return 0;
    }

    if (!swd_reset_2()) {
        return 0;
    }
		
    if (!swd_switch_2(0xB76D)) {
        return 0;
    }

    if (!swd_reset_2()) {
        return 0;
    }

    if (!swd_read_idcode_2(&tmp)) {
		Debug("Read IDCODE Fault 0x%.4X\r\n",tmp);
        return 0;
    }
		
	Debug("Read IDCODE 0x%.4X\r\n",tmp);
    return 1;
}

uint8_t swd_init_debug_2(void)
{
    uint32_t tmp = 0;
    int i = 0;
    uint8_t timeout = 100;
    // init dap state with fake values
    dap_state_2.select = 0xffffffff;
    dap_state_2.csw = 0xffffffff;
	
    swd_init_2();
	
    // call a target dependant function
    // this function can do several stuff before really initing the debug
    //target_before_init_debug();

    if (!JTAG2SWD_2()) {
		Debug("JTAG2SWD_1 Fault\r\n");
        return 0;
    }
		
    if (!swd_write_dp_2(DP_ABORT, STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR)) {
		Debug("swd_write_dp_2 Fault\r\n");
        return 0;
    }

    // Ensure CTRL/STAT register selected in DPBANKSEL
    if (!swd_write_dp_2(DP_SELECT, 0)) {
		Debug("swd_write_dp_2 DP_SELECT Fault\r\n");
        return 0;
    }

    // Power up
    if (!swd_write_dp_2(DP_CTRL_STAT, CSYSPWRUPREQ | CDBGPWRUPREQ)) {
		Debug("swd_write_dp_2 DP_CTRL_STAT Fault\r\n");
        return 0;
    }

    for (i = 0; i < timeout; i++) {
        if (!swd_read_dp_2(DP_CTRL_STAT, &tmp)) {
			Debug("swd_read_dp_2 DP_CTRL_STAT Fault\r\n");
            return 0;
        }
        if ((tmp & (CDBGPWRUPACK | CSYSPWRUPACK)) == (CDBGPWRUPACK | CSYSPWRUPACK)) {
            // Break from loop if powerup is complete
            break;
        }
    }
    if (i == timeout) {
		Debug("Unable to powerup DP\r\n");
        // Unable to powerup DP
        return 0;
    }

    if (!swd_write_dp_2(DP_CTRL_STAT, CSYSPWRUPREQ | CDBGPWRUPREQ | TRNNORMAL | MASKLANE)) {
		Debug("swd_write_dp_2 DP_CTRL_STAT Fault\r\n");
        return 0;
    }
    // call a target dependant function:
    // some target can enter in a lock state, this function can unlock these targets
    //target_unlock_sequence();

    if (!swd_write_dp_2(DP_SELECT, 0)) {
		Debug("swd_write_dp_2 DP_SELECT last Fault\r\n");
        return 0;
    }
    return 1;
}

void swd_set_target_reset_hw_2(uint8_t asserted)
{
    (asserted) ? PIN_nRESET_OUT_2(0) : PIN_nRESET_OUT_2(1);
}

void swd_set_target_reset_sw_2(void)
{
	if(g_AP_SEL_2 == M0_AP)
	{
		swd_write_word_2((uint32_t)&SCB->AIRCR, ((0x5FA << SCB_AIRCR_VECTKEY_Pos) |(SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) | SCB_AIRCR_SYSRESETREQ_Msk));
	}
	else
	{
		swd_write_word_2((uint32_t)&SCB->AIRCR, ((0x5FA << SCB_AIRCR_VECTKEY_Pos) |(SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) | SCB_AIRCR_VECTCLRACTIVE_Msk));
	}
}

uint8_t swd_set_target_state_hw_2(TARGET_RESET_STATE state)
{
    uint32_t val;
    int8_t ap_retries = 2;
    /* Calling swd_init_2 prior to entering RUN state causes operations to fail. */
    if (state != RUN) {
        swd_init_2();
    }

    switch (state) {
        case RESET_HOLD:
            swd_set_target_reset_hw_2(1);
            break;

        case RESET_RUN:
            swd_set_target_reset_hw_2(1);
            delay_ms(20);
            swd_set_target_reset_hw_2(0);
            delay_ms(20);
            swd_off_2();
            break;

        case RESET_PROGRAM:
            if (!swd_init_debug_2()) {
                return 0;
            }
												
            // Enable debug
            while(swd_write_word_2(DBG_HCSR, DBGKEY | C_DEBUGEN | C_HALT) == 0) 
			{
                if( --ap_retries <=0 )
				{
					Debug("write DBG_HCSR Fault\r\n");
					return 0;
				}                
            }
						
            do
			{
                if (!swd_read_word_2(DBG_HCSR, &val))
				{
					Debug("swd_read_word_2 Fault 0x%.4X\r\n",val);
					return 0;
                }	
				swd_set_target_reset_sw_2();
			} 
			while ((val & S_HALT) == 0);

            break;

        default:
            return 0;
    }

    return 1;
}

//********************************************************************************

static DAP_STATE dap_state_3;

static uint8_t swd_read_core_register_3(uint32_t n, uint32_t *val);
static uint8_t swd_write_core_register_3(uint32_t n, uint32_t val);

static void int2array_3(uint8_t *res, uint32_t data, uint8_t len)
{
    uint8_t i = 0;

    for(i = 0; i < len; i++) {
        res[i] = (data >> 8 * i) & 0xFF;
    }
}

static uint8_t swd_transfer_retry_3(uint32_t req, uint32_t *data)
{
    uint8_t i, ack=0;

    for (i = 0; i < MAX_SWD_RETRY; i++) 
	{
        ack = SWD_Transfer_3(req, data);
        if (ack != DAP_TRANSFER_WAIT) 
		{
            return ack;
        }
    }
    return ack;
}

uint8_t swd_init_3(void)
{
    DAP_Setup_3();
    PORT_SWD_SETUP_3();
	
    return 1;
}

uint8_t swd_off_3(void)
{
    PORT_OFF_3();
	
    return 1;
}

// Read debug port register.
uint8_t swd_read_dp_3(uint8_t adr, uint32_t *val)
{
    uint32_t tmp_in;
    uint8_t tmp_out[4];
    uint8_t ack;
    uint32_t tmp;
	
    tmp_in = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(adr);
    ack = swd_transfer_retry_3(tmp_in, (uint32_t *)tmp_out);
	
    *val = 0;
    tmp = tmp_out[3];
    *val |= (tmp << 24);
    tmp = tmp_out[2];
    *val |= (tmp << 16);
    tmp = tmp_out[1];
    *val |= (tmp << 8);
    tmp = tmp_out[0];
    *val |= (tmp << 0);
	
    return (ack == 0x01);
}

// Write debug port register
uint8_t swd_write_dp_3(uint8_t adr, uint32_t val)
{
    uint32_t req;
    uint8_t data[4];
    uint8_t ack;

    switch (adr) {
        case DP_SELECT:
            if (dap_state_3.select == val) {
                return 1;
            }

            dap_state_3.select = val;
            break;

        default:
            break;
    }

    req = SWD_REG_DP | SWD_REG_W | SWD_REG_ADR(adr);
    int2array_3(data, val, 4);
    ack = swd_transfer_retry_3(req, (uint32_t *)data);
	
    return (ack == 0x01);
}

// Read access port register.
uint8_t swd_read_ap_3(uint32_t adr, uint32_t *val)
{
    uint8_t tmp_in, ack;
    uint8_t tmp_out[4];
    uint32_t tmp;
    uint32_t apsel = adr & 0xff000000;
    uint32_t bank_sel = adr & APBANKSEL;

    if (!swd_write_dp_3(DP_SELECT, apsel | bank_sel)) {
        return 0;
    }

    tmp_in = SWD_REG_AP | SWD_REG_R | SWD_REG_ADR(adr);
    // first dummy read
    swd_transfer_retry_3(tmp_in, (uint32_t *)tmp_out);
    ack = swd_transfer_retry_3(tmp_in, (uint32_t *)tmp_out);
	
    *val = 0;
    tmp = tmp_out[3];
    *val |= (tmp << 24);
    tmp = tmp_out[2];
    *val |= (tmp << 16);
    tmp = tmp_out[1];
    *val |= (tmp << 8);
    tmp = tmp_out[0];
    *val |= (tmp << 0);
	
    return (ack == 0x01);
}

// Write access port register
uint8_t swd_write_ap_3(uint32_t adr, uint32_t val)
{
    uint8_t data[4];
    uint8_t req, ack;
    uint32_t apsel = adr & 0xff000000;
    uint32_t bank_sel = adr & APBANKSEL;
	
	if(g_AP_SEL_3 == M0_AP)
	{
		apsel |= 0x03000000;
	}

    if (!swd_write_dp_3(DP_SELECT, apsel | bank_sel)) {
        return 0;
    }

    switch (adr) {
        case AP_CSW:
            if (dap_state_3.csw == val) {
                return 1;
            }

            dap_state_3.csw = val;
            break;

        default:
            break;
    }

    req = SWD_REG_AP | SWD_REG_W | SWD_REG_ADR(adr);
    int2array_3(data, val, 4);

    if (swd_transfer_retry_3(req, (uint32_t *)data) != 0x01) {
        return 0;
    }

    req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
    ack = swd_transfer_retry_3(req, NULL);
	
    return (ack == 0x01);
}

// Write 32-bit word aligned values to target memory using address auto-increment.
// size is in bytes.
static uint8_t swd_write_block_3(uint32_t address, uint8_t *data, uint32_t size)
{
    uint8_t tmp_in[4], req;
    uint32_t size_in_words;
    uint32_t i, ack;

    if (size == 0) {
        return 0;
    }

    size_in_words = size / 4;

    // CSW register
    if (!swd_write_ap_3(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
        return 0;
    }

    // TAR write
    req = SWD_REG_AP | SWD_REG_W | (1 << 2);
    int2array_3(tmp_in, address, 4);

    if (swd_transfer_retry_3(req, (uint32_t *)tmp_in) != 0x01) {
        return 0;
    }

    // DRW write
    req = SWD_REG_AP | SWD_REG_W | (3 << 2);

    for (i = 0; i < size_in_words; i++) {
        if (swd_transfer_retry_3(req, (uint32_t *)data) != 0x01) {
            return 0;
        }

        data += 4;
    }

    // dummy read
    req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
    ack = swd_transfer_retry_3(req, NULL);
    return (ack == 0x01);
}

// Read 32-bit word aligned values from target memory using address auto-increment.
// size is in bytes.
static uint8_t swd_read_block_3(uint32_t address, uint8_t *data, uint32_t size)
{
    uint8_t tmp_in[4], req, ack;
    uint32_t size_in_words;
    uint32_t i;

    if (size == 0) {
        return 0;
    }

    size_in_words = size / 4;

    if (!swd_write_ap_3(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
        return 0;
    }

    // TAR write
    req = SWD_REG_AP | SWD_REG_W | AP_TAR;
    int2array_3(tmp_in, address, 4);

    if (swd_transfer_retry_3(req, (uint32_t *)tmp_in) != DAP_TRANSFER_OK) {
        return 0;
    }

    // read data
    req = SWD_REG_AP | SWD_REG_R | AP_DRW;

    // initiate first read, data comes back in next read
    if (swd_transfer_retry_3(req, NULL) != 0x01) {
        return 0;
    }

    for (i = 0; i < (size_in_words - 1); i++) {
        if (swd_transfer_retry_3(req, (uint32_t *)data) != DAP_TRANSFER_OK) {
            return 0;
        }

        data += 4;
    }

    // read last word
    req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
    ack = swd_transfer_retry_3(req, (uint32_t *)data);
    return (ack == 0x01);
}

// Read target memory.
uint8_t swd_read_data_3(uint32_t addr, uint32_t *val)
{
    uint8_t tmp_in[4];
    uint8_t tmp_out[4];
    uint8_t req, ack;
    uint32_t tmp;
    // put addr in TAR register
    int2array_3(tmp_in, addr, 4);
    req = SWD_REG_AP | SWD_REG_W | (1 << 2);

    if (swd_transfer_retry_3(req, (uint32_t *)tmp_in) != 0x01) {
        return 0;
    }

    // read data
    req = SWD_REG_AP | SWD_REG_R | (3 << 2);

    if (swd_transfer_retry_3(req, (uint32_t *)tmp_out) != 0x01) {
        return 0;
    }

    // dummy read
    req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
    ack = swd_transfer_retry_3(req, (uint32_t *)tmp_out);
    *val = 0;
    tmp = tmp_out[3];
    *val |= (tmp << 24);
    tmp = tmp_out[2];
    *val |= (tmp << 16);
    tmp = tmp_out[1];
    *val |= (tmp << 8);
    tmp = tmp_out[0];
    *val |= (tmp << 0);
    return (ack == 0x01);
}

// Write target memory.
uint8_t swd_write_data_3(uint32_t address, uint32_t data)
{
    uint8_t tmp_in[4];
    uint8_t req, ack;
    // put addr in TAR register
    int2array_3(tmp_in, address, 4);
    req = SWD_REG_AP | SWD_REG_W | (1 << 2);

    if (swd_transfer_retry_3(req, (uint32_t *)tmp_in) != 0x01) {
        return 0;
    }

    // write data
    int2array_3(tmp_in, data, 4);
    req = SWD_REG_AP | SWD_REG_W | (3 << 2);

    if (swd_transfer_retry_3(req, (uint32_t *)tmp_in) != 0x01) {
        return 0;
    }

    // dummy read
    req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
    ack = swd_transfer_retry_3(req, NULL);
    return ack;
}

// Read 32-bit word from target memory.
uint8_t swd_read_word_3(uint32_t addr, uint32_t *val)
{
    if (!swd_write_ap_3(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
        return 0;
    }
		
    if (!swd_read_data_3(addr, val)) {
        return 0;
    }
    return 1;
}

// Write 32-bit word to target memory.
uint8_t swd_write_word_3(uint32_t addr, uint32_t val)
{
    if (!swd_write_ap_3(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
        return 0;
    }


    if (!swd_write_data_3(addr, val)) {
        return 0;
    }
		
    return 1;
}

// Read 8-bit byte from target memory.
static uint8_t swd_read_byte_3(uint32_t addr, uint8_t *val)
{
    uint32_t tmp;

    if (!swd_write_ap_3(AP_CSW, CSW_VALUE | CSW_SIZE8)) {
        return 0;
    }

    if (!swd_read_data_3(addr, &tmp)) {
        return 0;
    }

    *val = (uint8_t)(tmp >> ((addr & 0x03) << 3));
    return 1;
}

// Write 8-bit byte to target memory.
static uint8_t swd_write_byte_3(uint32_t addr, uint8_t val)
{
    uint32_t tmp;

    if (!swd_write_ap_3(AP_CSW, CSW_VALUE | CSW_SIZE8)) {
        return 0;
    }

    tmp = (uint32_t)val << ((addr & 0x03) << 3);

    if (!swd_write_data_3(addr, tmp)) {
        return 0;
    }

    return 1;
}

// Read unaligned data from target memory.
// size is in bytes.
uint8_t swd_read_memory_3(uint32_t address, uint8_t *data, uint32_t size)
{
    uint32_t n;

    // Read bytes until word aligned
    while ((size > 0) && (address & 0x3)) {
        if (!swd_read_byte_3(address, data)) {
            return 0;
        }

        address++;
        data++;
        size--;
    }

    // Read word aligned blocks
    while (size > 3) {
        // Limit to auto increment page size
        n = Flash_Page_Size - (address & (Flash_Page_Size - 1));

        if (size < n) {
            n = size & 0xFFFFFFFC; // Only count complete words remaining
        }

        if (!swd_read_block_3(address, data, n)) {
            return 0;
        }

        address += n;
        data += n;
        size -= n;
    }

    // Read remaining bytes
    while (size > 0) {
        if (!swd_read_byte_3(address, data)) {
            return 0;
        }

        address++;
        data++;
        size--;
    }

    return 1;
}

// Write unaligned data to target memory.
// size is in bytes.
uint8_t swd_write_memory_3(uint32_t address, uint8_t *data, uint32_t size)
{
    uint32_t n = 0;

    // Write bytes until word aligned
    while ((size > 0) && (address & 0x3)) {
        if (!swd_write_byte_3(address, *data)) {
            return 0;
        }

        address++;
        data++;
        size--;
    }

    // Write word aligned blocks
    while (size > 3) {
        // Limit to auto increment page size
        n = Flash_Page_Size - (address & (Flash_Page_Size - 1));

        if (size < n) {
            n = size & 0xFFFFFFFC; // Only count complete words remaining
        }

        if (!swd_write_block_3(address, data, n)) {
            return 0;
        }

        address += n;
        data += n;
        size -= n;
    }

    // Write remaining bytes
    while (size > 0) {
        if (!swd_write_byte_3(address, *data)) {
            return 0;
        }

        address++;
        data++;
        size--;
    }

    return 1;
}

// Execute system call.
static uint8_t swd_write_debug_state_3(DEBUG_STATE *state)
{
    uint32_t i, status;

    if (!swd_write_dp_3(DP_SELECT, 0)) {
        return 0;
    }

    // R0, R1, R2, R3
    for (i = 0; i < 4; i++) {
        if (!swd_write_core_register_3(i, state->r[i])) {
            return 0;
        }
    }

    // R9
    if (!swd_write_core_register_3(9, state->r[9])) {
        return 0;
    }

    // R13, R14, R15
    for (i = 13; i < 16; i++) {
        if (!swd_write_core_register_3(i, state->r[i])) {
            return 0;
        }
    }

    // xPSR
    if (!swd_write_core_register_3(16, state->xpsr)) {
        return 0;
    }

    if (!swd_write_word_3(DBG_HCSR, DBGKEY | C_DEBUGEN)) {
        return 0;
    }

    // check status
    if (!swd_read_dp_3(DP_CTRL_STAT, &status)) {
        return 0;
    }

    if (status & (STICKYERR | WDATAERR)) {
        return 0;
    }

    return 1;
}

static uint8_t swd_read_core_register_3(uint32_t n, uint32_t *val)
{
    int i = 0, timeout = 100;

    if (!swd_write_word_3(DCRSR, n)) {
        return 0;
    }

    // wait for S_REGRDY
    for (i = 0; i < timeout; i++) {
        if (!swd_read_word_3(DHCSR, val)) {
            return 0;
        }

        if (*val & S_REGRDY) {
            break;
        }
    }

    if (i == timeout) {
        return 0;
    }

    if (!swd_read_word_3(DCRDR, val)) {
        return 0;
    }

    return 1;
}

static uint8_t swd_write_core_register_3(uint32_t n, uint32_t val)
{
    int i = 0, timeout = 100;

    if (!swd_write_word_3(DCRDR, val)) {
        return 0;
    }

    if (!swd_write_word_3(DCRSR, n | REGWnR)) {
        return 0;
    }

    // wait for S_REGRDY
    for (i = 0; i < timeout; i++) {
        if (!swd_read_word_3(DHCSR, &val)) {
            return 0;
        }

        if (val & S_REGRDY) {
            return 1;
        }
    }

    return 0;
}

static uint8_t swd_wait_until_halted_3(void)
{
    // Wait for target to stop
    uint32_t val, i, timeout = MAX_TIMEOUT;
	
	#ifdef exec_try
		if (syscall_exec_try_3 == 1)
		{
			timeout = 5; 	
		}
	#endif

    for (i = 0; i < timeout; i++) {
		
        if (!swd_read_word_3(DBG_HCSR, &val)) {
            return 0;
        }

        if (val & S_HALT) {
			
			#ifdef exec_try
				syscall_exec_try_3 = 0;	
			#endif
			
            return 1;
        }
    }

    return 0;
}

uint8_t swd_flash_syscall_exec_3(const program_syscall_t *sysCallParam, uint32_t entry, uint32_t arg1, uint32_t arg2, uint32_t arg3, uint32_t arg4)
{
    DEBUG_STATE state = {{0}, 0};
    // Call flash algorithm function on target and wait for result.
    state.r[0]     = arg1;                   // R0: Argument 1
    state.r[1]     = arg2;                   // R1: Argument 2
    state.r[2]     = arg3;                   // R2: Argument 3
    state.r[3]     = arg4;                   // R3: Argument 4
    state.r[9]     = sysCallParam->static_base;    // SB: Static Base
    state.r[13]    = sysCallParam->stack_pointer;  // SP: Stack Pointer
    state.r[14]    = sysCallParam->breakpoint;     // LR: Exit Point
    state.r[15]    = entry;                        // PC: Entry Point
    state.xpsr     = 0x01000000;          // xPSR: T = 1, ISR = 0
		
    if (!swd_write_debug_state_3(&state)) {
		Debug("Write State Faulf\r\n");
        return 0;
    }

    if (!swd_wait_until_halted_3()) {
		#ifdef exec_try
			if (syscall_exec_try_3 == 1)
			{
				syscall_exec_try_3 = 0;	
				
				return 0;
			}
			else
			{
				Debug("swd_wait_until_halted_3 Faulf\r\n");
				
				return 0;
			}	
		#endif
		
		Debug("swd_wait_until_halted_3 Faulf\r\n");
			
        return 0;
    }

    if (!swd_read_core_register_3(0, &state.r[0])) {
		Debug("swd_read_core_register_3 Faulf\r\n");
        return 0;
    }

    // Flash functions return 0 if successful.
    if (state.r[0] != 0) {
		Debug(" Fault 0x%.4X\r\n",state.r[0]);
       return 0;
    }

    return 1;
}

// SWD Reset
static uint8_t swd_reset_3(void)
{
    uint8_t tmp_in[8];
    uint8_t i = 0;

    for (i = 0; i < 8; i++) {
        tmp_in[i] = 0xff;
    }

    SWD_Sequence_3(51, tmp_in);
    return 1;
}

// SWD Switch
static uint8_t swd_switch_3(uint16_t val)
{
    uint8_t tmp_in[2];
    tmp_in[0] = val & 0xff;
    tmp_in[1] = (val >> 8) & 0xff;
    SWD_Sequence_3(16, tmp_in);
    return 1;
}

// SWD Read ID
static uint8_t swd_read_idcode_3(uint32_t *id)
{
    uint8_t tmp_in[1];
    uint8_t tmp_out[4];
    tmp_in[0] = 0x00;
    SWD_Sequence_3(8, tmp_in);
	
    if (swd_read_dp_3(0, (uint32_t *)tmp_out) != 0x01) {
        return 0;
    }

    *id = (tmp_out[3] << 24) | (tmp_out[2] << 16) | (tmp_out[1] << 8) | tmp_out[0];
    return 1;
}

static uint8_t JTAG2SWD_3()
{
    uint32_t tmp = 0;

    if (!swd_reset_3()) {
        return 0;
    }

    if (!swd_switch_3(0xE79E)) {
        return 0;
    }

    if (!swd_reset_3()) {
        return 0;
    }
		
    if (!swd_switch_3(0xB76D)) {
        return 0;
    }

    if (!swd_reset_3()) {
        return 0;
    }

    if (!swd_read_idcode_3(&tmp)) {
		Debug("Read IDCODE Fault 0x%.4X\r\n",tmp);
        return 0;
    }
		
	Debug("Read IDCODE 0x%.4X\r\n",tmp);
    return 1;
}

uint8_t swd_init_debug_3(void)
{
    uint32_t tmp = 0;
    int i = 0;
    uint8_t timeout = 100;
    // init dap state with fake values
    dap_state_3.select = 0xffffffff;
    dap_state_3.csw = 0xffffffff;
	
    swd_init_3();
	
    // call a target dependant function
    // this function can do several stuff before really initing the debug
    //target_before_init_debug();

    if (!JTAG2SWD_3()) {
		Debug("JTAG2SWD Fault\r\n");
        return 0;
    }
		
    if (!swd_write_dp_3(DP_ABORT, STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR)) {
		Debug("swd_write_dp_3 Fault\r\n");
        return 0;
    }

    // Ensure CTRL/STAT register selected in DPBANKSEL
    if (!swd_write_dp_3(DP_SELECT, 0)) {
		Debug("swd_write_dp_3 DP_SELECT Fault\r\n");
        return 0;
    }

    // Power up
    if (!swd_write_dp_3(DP_CTRL_STAT, CSYSPWRUPREQ | CDBGPWRUPREQ)) {
		Debug("swd_write_dp_3 DP_CTRL_STAT Fault\r\n");
        return 0;
    }

    for (i = 0; i < timeout; i++) {
        if (!swd_read_dp_3(DP_CTRL_STAT, &tmp)) {
			Debug("swd_read_dp_3 DP_CTRL_STAT Fault\r\n");
            return 0;
        }
        if ((tmp & (CDBGPWRUPACK | CSYSPWRUPACK)) == (CDBGPWRUPACK | CSYSPWRUPACK)) {
            // Break from loop if powerup is complete
            break;
        }
    }
    if (i == timeout) {
		Debug("Unable to powerup DP\r\n");
        // Unable to powerup DP
        return 0;
    }

    if (!swd_write_dp_3(DP_CTRL_STAT, CSYSPWRUPREQ | CDBGPWRUPREQ | TRNNORMAL | MASKLANE)) {
		Debug("swd_write_dp_3 DP_CTRL_STAT Fault\r\n");
        return 0;
    }
    // call a target dependant function:
    // some target can enter in a lock state, this function can unlock these targets
    //target_unlock_sequence();

    if (!swd_write_dp_3(DP_SELECT, 0)) {
		Debug("swd_write_dp_3 DP_SELECT last Fault\r\n");
        return 0;
    }
    return 1;
}

void swd_set_target_reset_hw_3(uint8_t asserted)
{
    (asserted) ? PIN_nRESET_OUT_3(0) : PIN_nRESET_OUT_3(1);
}

void swd_set_target_reset_sw_3(void)
{
	if(g_AP_SEL_3 == M0_AP)
	{
		swd_write_word_3((uint32_t)&SCB->AIRCR, ((0x5FA << SCB_AIRCR_VECTKEY_Pos) |(SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) | SCB_AIRCR_SYSRESETREQ_Msk));
	}
	else
	{
		swd_write_word_3((uint32_t)&SCB->AIRCR, ((0x5FA << SCB_AIRCR_VECTKEY_Pos) |(SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) | SCB_AIRCR_VECTCLRACTIVE_Msk));
	}
}

uint8_t swd_set_target_state_hw_3(TARGET_RESET_STATE state)
{
    uint32_t val;
    int8_t ap_retries = 2;
    /* Calling swd_init_3 prior to entering RUN state causes operations to fail. */
    if (state != RUN) {
        swd_init_3();
    }

    switch (state) {
        case RESET_HOLD:
            swd_set_target_reset_hw_3(1);
            break;

        case RESET_RUN:
            swd_set_target_reset_hw_3(1);
            delay_ms(20);
            swd_set_target_reset_hw_3(0);
            delay_ms(20);
            swd_off_3();
            break;

        case RESET_PROGRAM:
            if (!swd_init_debug_3()) {
                return 0;
            }
												
            // Enable debug
            while(swd_write_word_3(DBG_HCSR, DBGKEY | C_DEBUGEN | C_HALT) == 0) 
			{
                if( --ap_retries <=0 )
				{
					Debug("write DBG_HCSR Fault\r\n");
					return 0;
				}                
            }
						
            do
			{
                if (!swd_read_word_3(DBG_HCSR, &val))
				{
					Debug("swd_read_word_3 Fault 0x%.4X\r\n",val);
					return 0;
                }	
				swd_set_target_reset_sw_3();
			} 
			while ((val & S_HALT) == 0);

            break;

        default:
            return 0;
    }

    return 1;
}


//********************************************************************************

